#include <ros/ros.h>
#include <std_msgs/Int32.h>
#include <gpiod.h>
#include <thread>
#include <vector>
#include <map>
#include <termios.h>
#include <unistd.h>
#include <XmlRpcValue.h>

// // 获取指定 GPIO 对应的 chip 和 line
// bool get_line_for_gpio(int gpio_num, gpiod_chip** chip_out, gpiod_line** line_out) {
//     // 遍历所有 gpiochipX
//     for (int chip_idx = 0; chip_idx <= 5; ++chip_idx) {
//         std::string chip_name = "gpiochip" + std::to_string(chip_idx);
//         gpiod_chip* chip = gpiod_chip_open_by_name(chip_name.c_str());
//         if (!chip) continue;

//         int offset = gpio_num - gpiod_chip_get_line_offset(chip, 0);
//         if (offset < 0 || offset >= gpiod_chip_num_lines(chip)) {
//             gpiod_chip_close(chip);
//             continue;
//         }

//         gpiod_line* line = gpiod_chip_get_line(chip, offset);
//         if (!line) {
//             gpiod_chip_close(chip);
//             continue;
//         }

//         *chip_out = chip;
//         *line_out = line;
//         return true;
//     }
//     return false;
// }


class GpioMultiInterrupt {
public:
  struct PinConfig {
    int pin;
    bool rising;
    bool falling;
  };
  std::map<int, gpiod_chip*> chip_map_;  // key: chip_index，value: gpiod_chip*
  // 输出脚配置
  struct OutPinConfig {
    int  pin;          // 仍沿用你的全局 pin 编号
    int  initial = 0;  // 0/1 逻辑电平（未考虑 active_low 前）
    bool active_low = false;
  };

  std::vector<OutPinConfig> out_cfgs_;
  std::map<int, gpiod_line*> out_lines_;   // pin -> line*
  std::map<int, bool>        out_alow_;    // pin -> active_low
  ros::Subscriber            out_sub_;
  std::mutex                 out_mtx_;

  GpioMultiInterrupt()
      : nh_("~")
  {
    XmlRpc::XmlRpcValue cfg_list;
    if (!ros::param::get("pin_configs", cfg_list) || cfg_list.getType() != XmlRpc::XmlRpcValue::TypeArray) {
        ROS_FATAL("请通过 pin_configs 参数传入 GPIO 配置列表");
        ros::shutdown();
        return;
    }

    for (int i = 0; i < cfg_list.size(); ++i) {
        auto& entry = cfg_list[i];
        PinConfig cfg;
        cfg.pin = static_cast<int>(entry["pin"]);
        cfg.rising = static_cast<bool>(entry["rising"]);
        cfg.falling = static_cast<bool>(entry["falling"]);
        pins_.push_back(cfg);
    }

    pub_ = nh_.advertise<std_msgs::Int32>("/gpio_event", 100);
    pub_out_ = nh_.advertise<std_msgs::Int32>("/gpio_set", 100);

    for (const auto& cfg : pins_) {
        int chip_index = cfg.pin / 32;
        int line_offset = cfg.pin % 32;
        std::string chip_name = "gpiochip" + std::to_string(chip_index);

        gpiod_chip* chip = nullptr;
        if (chip_map_.count(chip_index)) {
            chip = chip_map_[chip_index];
        } else {
            chip = gpiod_chip_open_by_name(chip_name.c_str());
            if (!chip) {
                ROS_ERROR("1234打开 %s 失败，GPIO %d 无法监听", chip_name.c_str(), cfg.pin);
                continue;
            }
            chip_map_[chip_index] = chip;
        }

        gpiod_line* line = gpiod_chip_get_line(chip, line_offset);
        if (!line) {
            ROS_ERROR("5678获取 GPIO %d (chip=%s offset=%d) 失败", cfg.pin, chip_name.c_str(), line_offset);
            continue;
        }

        int request_type = 0;
        if (cfg.rising && cfg.falling) {
            request_type = GPIOD_LINE_REQUEST_EVENT_BOTH_EDGES;
        } else if (cfg.rising) {
            request_type = GPIOD_LINE_REQUEST_EVENT_RISING_EDGE;
        } else { // 已保证 event_flags != 0
            request_type = GPIOD_LINE_REQUEST_EVENT_FALLING_EDGE;
        }


        if (request_type == 0) {
            ROS_WARN("GPIO %d 未启用 rising 或 falling 边沿，跳过", cfg.pin);
            continue;
        }

        struct gpiod_line_request_config req = {
            .consumer = "gpio_event_monitor",
            .request_type = request_type,
            .flags = 0
        };

        if (gpiod_line_request(line, &req, 0) < 0) {
            perror("gpiod_line_request 失败");
            ROS_ERROR("GPIO %d 申请失败", cfg.pin);
            continue;
        }

        lines_.push_back(line);
        pin_map_[line] = cfg.pin;

        ROS_INFO("Monitored GPIO %d:%s", cfg.pin,
            (request_type == GPIOD_LINE_REQUEST_EVENT_BOTH_EDGES ? "BOTH" :
             request_type == GPIOD_LINE_REQUEST_EVENT_RISING_EDGE ? "RISING" :
             request_type == GPIOD_LINE_REQUEST_EVENT_FALLING_EDGE ? "FALLING" : "MIXED"));
    }
    
    XmlRpc::XmlRpcValue out_list;
    if (ros::param::get("output_configs", out_list) && 
        out_list.getType() == XmlRpc::XmlRpcValue::TypeArray) {
      for (int i = 0; i < out_list.size(); ++i) {
        auto& e = out_list[i];
        if (!e.hasMember("pin")) {
          ROS_ERROR("output_configs[%d] 缺少 pin 字段，已跳过", i);
          continue;
        }
        OutPinConfig oc;
        oc.pin        = static_cast<int>(e["pin"]);
        oc.initial    = e.hasMember("initial")    ? static_cast<int>(e["initial"])     : 0;
        oc.active_low = e.hasMember("active_low") ? static_cast<bool>(e["active_low"]) : false;
        out_cfgs_.push_back(oc);
      }
      for (const auto& cfg : out_cfgs_) {
        int chip_index = cfg.pin / 32;                  // ⚠️ 同你现有逻辑（跨平台建议改 chip+offset）
        int line_offset = cfg.pin % 32;
        std::string chip_name = "gpiochip" + std::to_string(chip_index);

        gpiod_chip* chip = nullptr;
        if (chip_map_.count(chip_index)) chip = chip_map_[chip_index];
        else {
          chip = gpiod_chip_open_by_name(chip_name.c_str());
          if (!chip) { ROS_ERROR("打开 %s 失败，GPIO %d 无法作为输出", chip_name.c_str(), cfg.pin); continue; }
          chip_map_[chip_index] = chip;
        }

        gpiod_line* line = gpiod_chip_get_line(chip, line_offset);
        if (!line) { ROS_ERROR("获取 GPIO %d (chip=%s offset=%d) 失败", cfg.pin, chip_name.c_str(), line_offset); continue; }

        // 逻辑->物理电平（active_low 由软件反相）
        int logical_init = cfg.initial ? 1 : 0;
        int phys_init    = cfg.active_low ? (logical_init ^ 1) : logical_init;

        if (gpiod_line_request_output(line, "gpio_output", phys_init) < 0) {
          perror("gpiod_line_request_output 失败");
          ROS_ERROR("GPIO %d 申请输出失败", cfg.pin);
          continue;
        }

        out_lines_[cfg.pin] = line;
        out_alow_[cfg.pin]  = cfg.active_low;
        ROS_INFO("Configured GPIO %d as OUTPUT (init=%d, active_low=%s)", cfg.pin, cfg.initial, cfg.active_low ? "true" : "false");
      }

      // 订阅设置话题：正数=置高，负数=置低（例如 17→高，-17→低）
      out_sub_ = nh_.subscribe<std_msgs::Int32>("/gpio_set", 10, &GpioMultiInterrupt::onGpioSet, this);
    }
    if (!lines_.empty()) {
        event_thread_ = std::thread(&GpioMultiInterrupt::eventLoop, this);
    } else {
        ROS_FATAL("无有效 GPIO 可监听，退出节点");
        ros::shutdown();
    }
  }

  void spin() {
    while (ros::ok()) {
      char ch = getKey();
      if (ch == 'm') {
        std::cout << "[用户操作] Manual control\n";
      } else {
        std::cout << "[警告] 无效按键 '" << ch << "'\n";
      }
      ros::spinOnce();
    }
  }

  ~GpioMultiInterrupt() {
    running_ = false;
    for (auto& kv : out_lines_) {
      if (kv.second) gpiod_line_release(kv.second);
    }

    if (event_thread_.joinable()) event_thread_.join();
    for (auto line : lines_) {
      gpiod_line_release(line);
    }
    for (auto& pair : chip_map_) {
        gpiod_chip_close(pair.second);
    }
    if (event_thread_.joinable()) {
        event_thread_.join();
    }
  }

private:
  ros::NodeHandle nh_;
  ros::Publisher pub_;
  ros::Publisher pub_out_;
  std::vector<PinConfig> pins_;
  std::vector<gpiod_line*> lines_;
  std::map<gpiod_line*, int> pin_map_;
  gpiod_chip* chip_ = nullptr;
  std::thread event_thread_;
  bool running_ = true;
  // void onGpioSet(const std_msgs::Int32::ConstPtr& msg);

  void onGpioSet(const std_msgs::Int32::ConstPtr& msg) {
    int encoded = msg->data;
    int pin     = std::abs(encoded);
    int logical = (encoded >= 0) ? 1 : 0;   // >=0 表示置高；<0 表示置低
    ROS_INFO("GPIO %d <= %d", pin, logical);
    auto it = out_lines_.find(pin);
    if (it == out_lines_.end()) {
      ROS_WARN("未配置为输出的 GPIO %d", pin);
      return;
    }

    bool alow = out_alow_[pin];
    int phys  = alow ? (logical ^ 1) : logical;

    std::lock_guard<std::mutex> lk(out_mtx_);
    if (gpiod_line_set_value(it->second, phys) < 0) {
      perror("gpiod_line_set_value 失败");
      ROS_ERROR("GPIO %d 写入失败", pin);
    } else {
      ROS_INFO("GPIO %d <= %d (logical=%d, active_low=%s)", pin, phys, logical, alow ? "true" : "false");
    }
  }

  char getKey() {
    struct termios oldt, newt;
    char ch;
    tcgetattr(STDIN_FILENO, &oldt);
    newt = oldt;
    newt.c_lflag &= ~(ICANON | ECHO);
    tcsetattr(STDIN_FILENO, TCSANOW, &newt);
    ch = getchar();
    tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
    return ch;
  }

  void eventLoop() {
    struct gpiod_line_event ev;
    struct timespec timeout;
    timeout.tv_sec = 0;
    timeout.tv_nsec = 10 * 1000 * 1000; // 100ms 超时

    std::map<gpiod_line*, ros::Time> last_trigger_time; // 记录每个引脚上次触发时间
    std::map<gpiod_line*, int> last_level_;             // 记录每个引脚上次电平
    ros::Duration debounce_time(0.5);                   // 设置去抖间隔 0.2 秒

    while (running_) {
        for (auto line : lines_) {
            int ret = gpiod_line_event_wait(line, &timeout);
            if (ret == 1 && gpiod_line_event_read(line, &ev) == 0) {
                // 用事件自带时间戳更准确
                ros::Time now(ev.ts.tv_sec, ev.ts.tv_nsec);

                const bool is_rising  = (ev.event_type == GPIOD_LINE_EVENT_RISING_EDGE);
                const bool is_falling = (ev.event_type == GPIOD_LINE_EVENT_FALLING_EDGE);
                int gpio_flag = 0;
                // std::cout<<"---->rising falling: "<<is_rising<<","<<is_falling<<std::endl;
                // 按“引脚+边沿”做去抖：key=(line, 1) 表示上升沿；(line, 0) 表示下降沿
                static std::map<std::pair<gpiod_line*, int>, ros::Time> last_ts;
                static std::map<gpiod_line*, ros::Time> last_ts_any;
                static std::map<gpiod_line*, int> last_edge; // -1: 未知, 0: FALLING, 1: RISING
                int edge = is_rising ? 1 : 0;
                auto it_inserted = last_edge.insert({ line, 1 });  // 已存在则不改
                int prev = it_inserted.first->second;

                auto it = last_edge.find(line);
                auto key = std::make_pair(line, is_rising ? 1 : 0);
                if (prev != edge){
                  it_inserted.first->second = edge;
                  if (!last_ts_any.count(line) || (now - last_ts_any[line]) > debounce_time) {
                      std::cout << last_ts_any[line] <<std::endl;
                      last_ts_any[line] = now;
                      std::cout << now <<std::endl;

                      int pin = pin_map_[line];
                      std_msgs::Int32 msg;
                      std_msgs::Int32 msg_out;

                      // 用正负号区分边沿：+pin=上升沿，-pin=下降沿（若不需要区分，改成 msg.data = pin 即可）
                      msg.data = is_rising ? pin : -pin;
                      pub_.publish(msg);
                      if (pin == 123){
                        if (msg.data < 0)
                          msg_out.data = 134;
                        else 
                          msg_out.data = -134;
                        pub_out_.publish(msg_out);
                      }
                      if (pin == 47){
                        msg_out.data =134;
                        pub_out_.publish(msg_out);
                      }


                      ROS_INFO("[GPIO interrupt1] GPIO %d %s",
                              pin, is_rising ? "RISING (0->1)" : "FALLING (1->0)");
                  }
                }

                // （可选）把队列里积压的事件清空
                // struct timespec zero{0,0};
                // while (gpiod_line_event_wait(line, &zero) == 1 &&
                //     gpiod_line_event_read(line, &ev) == 0) {
                //     // 如需，也可对积压事件重复同样处理
                //     now = ros::Time(ev.ts.tv_sec, ev.ts.tv_nsec);
                //     const bool rising2 = (ev.event_type == GPIOD_LINE_EVENT_RISING_EDGE);
                //     auto key2 = std::make_pair(line, rising2 ? 1 : 0);
                //     if (!last_ts.count(key2) || (now - last_ts[key2]) > debounce_time) {
                //         last_ts[key2] = now;
                //         int pin = pin_map_[line];
                //         std_msgs::Int32 msg;
                //         msg.data = rising2 ? pin : -pin;
                //         pub_.publish(msg);
                //         ROS_INFO("[GPIO interrupt2] GPIO %d %s",
                //                 pin, rising2 ? "RISING (0->1)" : "FALLING (1->0)");
                //     }
                // }
            }else if (ret < 0) {
                perror("gpiod_line_event_wait 错误");
            }
        }
        usleep(1000); // 降低 CPU 占用
    }
  }



};

int main(int argc, char** argv) {
  ros::init(argc, argv, "gpio_multi_interrupt");
  GpioMultiInterrupt node;

  ros::AsyncSpinner spinner(1);  // 1 个回调线程够用了
  spinner.start();

  // 如果你还要保留键盘交互，就继续跑 node.spin()（哪怕它阻塞也没关系）
  node.spin();

  return 0;
}

